﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Configuration;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ayxjDllClass
{
    public class CANProtocel
    {
        static unsafe void PushCmd(uint id, byte[] bytes)
        {
            VCI_CAN_OBJ pSend = new VCI_CAN_OBJ
            {
                ID = (uint)id,
                RemoteFlag = (byte)0,
                ExternFlag = (byte)0,
                DataLen = (byte)bytes.Length,
            };
            for (int i = 0; i < bytes.Length; i++)
            {
                pSend.Data[i] = bytes[i];
            }
            AxisControl.canQueue.Enqueue(pSend);
        }

        public static int  EnabledNum= 2;

        /// <summary>
        /// 设置电机使能
        /// </summary>
        /// <returns></returns>
        static int SetEnableByte()
        {
            switch (EnabledNum)
            { 
                case 0: return 32;
                case 1: return 16;
                case 2: return 8;
                    default: return 0;
            }
        }

        /// <summary>
        /// 电机使能
        /// </summary>
        /// <param name="Axis">0 方位；1 俯仰；2 极化</param>
        /// <param name="Enable"></param>
        /// <returns></returns>
        public static bool SetEnable(short Axis, bool Enable)
        {
            byte[] bytes = new byte[2];
            bytes[0] = (byte)0x82;
            bytes[1] = (byte)(128 + SetEnableByte());

            PushCmd((uint)Axis, bytes);
            return true;          
        }

        /// <summary>
        /// 减速比
        /// </summary>
        static int[] sb = new int[3] { 15700, 15700, 10 };

        /// <summary>
        /// 电机转/停
        /// </summary>
        /// <param name="Axis">0 方位；1 俯仰；2 极化</param>
        /// <param name="Direction">轴运动，0停止，1顺转，2逆转</param>
        /// <returns></returns>
        public static bool SetMove(short Axis, short Direction)
        {
            bool cancel = false;
            int speed = (int)(sb[Axis] * 1000 * AxisControl.Speed[Axis]);
            switch (Direction)
            {
                case 0: 
                    speed = 0;
                    AxisControl.poT[Axis] = AxisControl.boolRead4[Axis] = AxisControl.boolRead5[Axis] = false;
                    break;
                case 1:
                    //AxisControl.RLimitState [3,2]的数组，[x,0]是角度上限触发,[x,1]是角度下限触发
                    //如果是方位顺转，角度逐渐变大、需要判断是否触发了上限
                    //如果是俯仰顺转，角度逐渐变小、需要判断是否触发了下限
                    if (AxisControl.RLimitState[Axis, Axis==1?1: 0] != 1)
                    {
                        speed = -1 * speed;
                        AxisControl.boolRead4[Axis] = true;
                    }
                    else cancel = true;
                    break;
                case 2:
                    if (AxisControl.RLimitState[Axis, Axis == 1 ? 0:1] != 2)
                    {
                        speed = 1 * speed;
                        AxisControl.boolRead5[Axis] = true;
                    }
                    else cancel = true;
                    break;
            }
            if (cancel)
                return true;

            List<byte> bytes = new List<byte>();
            bytes.Add((byte)0x83);
            bytes.AddRange(speed.FromInt32());

            bytes.Add((byte)EnabledNum);
            PushCmd((uint)Axis, bytes.ToArray());
            return true;
        }

        /// <summary>
        /// 开启主动上报
        /// </summary>
        /// <param name="Axis">0 方位；1 俯仰；2 极化</param>
        /// <param name="enabled">true：开启；false：关闭。默认开启</param>
        /// <returns></returns>
        public static bool SetAutoReport(short Axis, bool enabled = true)
        {
            List<byte> bytes = new List<byte> { (byte)0x97 };
            bytes.Add((byte)(enabled ? 0x81 : 0x01));
            bytes.AddRange(new List<byte> { 0x64, 0, 0, 0, 0, 0 });
            PushCmd((uint)Axis, bytes.ToArray());
            return true;
        }

        /// <summary>
        /// 解析接收到的报文
        /// </summary>
        /// <param name="id">0 方位；1 俯仰；2 极化</param>
        /// <param name="len"></param>
        /// <param name="bytes"></param>
        public static void AnalyseCanData(uint id, byte len, byte[] bytes)
        {
            int index = (int)id;
            try
            {
                switch (bytes[0])
                {
                    case 0x20:
                        //1、2 字节：电机 1 当前速度(0.1rpm)。 
                        //3、4 字节：电机 2 当前速度(0.1rpm)。
                        ZtInfo.DriveList[index].Speed1 = Math.Abs(bytes.ToInt16(1) * 0.1); 
                        ZtInfo.DriveList[index].Speed2 = Math.Abs(bytes.ToInt16(3) * 0.1);
                        ZtInfo.DriveList[index].Machine2State1 = bytes[5];

                        //Console.WriteLine($"id:{index}  Speed1:{ZtInfo.DriveList[index].Speed1}  Speed2：{ZtInfo.DriveList[index].Speed2}");

                        // 电机转速以两个电机中转速大的为准（rpm）
                        double sd = ZtInfo.DriveList[index].Speed1 > ZtInfo.DriveList[index].Speed2 ? ZtInfo.DriveList[index].Speed1 : ZtInfo.DriveList[index].Speed2;

                        // 转每分 转 度每秒
                        // 1 rpm = 360du/min == 6du/s
                        AxisControl.SpeedNum[index] = 6 * sd / sb[index];
                        Console.WriteLine($"id:{index}  data:{BitConverter.ToString(bytes)}  速度：{sd}    RPM:{AxisControl.SpeedNum[index]}" );

                        break;
                    case 0x21:
                        //1、2 字节：电机 1 故障、状态。 
                        //3、4 字节：电机 2 故障、状态。 
                        ZtInfo.DriveList[index].Machine1Error0 = bytes[1];
                        ZtInfo.DriveList[index].Machine1Error1 = bytes[2];
                        ZtInfo.DriveList[index].Machine1State0 = bytes[3];
                        ZtInfo.DriveList[index].Machine1State1 = bytes[4];
                        ZtInfo.DriveList[index].Machine2Error0 = bytes[5];
                        ZtInfo.DriveList[index].Machine2Error1 = bytes[6];
                        ZtInfo.DriveList[index].Machine2State0 = bytes[7];
                        break;
                    case 0x02:
                        //使能控制字设置 / 查询
                        break;
                    case 0x03:
                        //速度设置 / 查询
                        //1、2、3、4字节：速度（0.1rpm）/ 电流（0.01A）。
                        //（速度模式 / 电流模式），5字节：电机选择（0：消隙时总电机合成，1：左电机，2：右电机）
                        break;
                    case 0x13:
                    case 0xA3:
                        //最大加速度设置 / 查询
                        //1、2、3、4字节：最大加速度（）
                        break;
                    case 0x97:
                        //主动上报设置，上电默认不上报，最小间隔单位为2ms，请确认下，最快间隔
                        break;
                }
            }
            catch { }
        }
    }

    public class DriveInfo
    {
        #region 上报报文解析
        // 2、	0x20电机的当前速度
        //	0x21 4字节的电机故障状态代码，怎么解析
        //	21 04 00 CC 04 00 00 83
        //	20 00 00 0A 40 00 00 01

        //0x21数据帧：	21 04 00 CC 04 00 00 83
        //	1到2字节（04 00）：电机1的故障码；
        //	3到4字节（CC 04）：电机1的状态码；
        //	5到6字节（00 00）：电机2的故障码；
        //	7字节（83）：电机2状态码低字节；

        //0x20数据帧：20 00 00 0A 40 00 00 01
        //	1到2字节（00 00）：电机1的速度；
        //	3到4字节（0A 40）：电机2的速度；
        //	5字节（00）：电机2状态码高字节；
        //	6字节（00）：电机1使能状态（使能：01；非使能：00）
        //	7字节（01）：电机2使能状态（使能：0A；非使能：01）
        #endregion

        public double Speed1 { get; set; }
        public double Speed2 { get; set; }

        /// <summary>
        /// 电机1故障码低 0x21数据帧 1字节
        /// </summary>
        public byte Machine1Error0 { get; set; }
        /// <summary>
        /// 电机1故障码高 0x21数据帧 2字节
        /// </summary>
        public byte Machine1Error1 { get; set; }
        /// <summary>
        /// 电机1状态码低 0x21数据帧 3字节
        /// </summary>
        public byte Machine1State0 { get; set; }
        /// <summary>
        /// 电机1状态码高 0x21数据帧 4字节
        /// </summary>
        public byte Machine1State1 { get; set; }
        /// <summary>
        /// 电机2故障码低 0x21数据帧 5字节
        /// </summary>
        public byte Machine2Error0 { get; set; }
        /// <summary>
        /// 电机2故障码高 0x21数据帧 6字节
        /// </summary>
        public byte Machine2Error1 { get; set; }
        /// <summary>
        /// 电机2状态码低 0x21数据帧 7字节
        /// </summary>
        public byte Machine2State0 { get; set; }
        /// <summary>
        /// 电机2状态码高 0x20数据帧 5字节
        /// </summary>
        public byte Machine2State1 { get; set; }
    }

    /// <summary>
    /// 6.2米转台的 方位、俯仰、极化 驱动器信息
    /// </summary>
    public class ZtInfo
    {
        public static List<DriveInfo> DriveList { get; set; } = new List<DriveInfo>() { new DriveInfo(), new DriveInfo(), new DriveInfo() };
    }
}
